#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include "myjni.h"
#include "platform_config.h"

#define MAX_AUTO_POINTS 10
#define  AUTO_VALID_DEPTH_MM    50

extern int g_depth;
static struct timeval auto_calc_tv1, auto_calc_tv2;
static int harvesterStateInvalid = 0;
static char auto_area_start_flags = 0;
static struct timeval auto_area_start_time;

int g_continuousNoWorkPointNums = 0;
double g_totalDistances_auto = 0;
double g_oldBearing = 0;
int manualDepthBuff[GPS_MAX_DATA];
unsigned int  g_manualDepthCounter = 0;
static int GpsDataStart=0;
static int combufindex=0;
stru_GPSPOINT g_corePoint;
stru_peakPoint g_peakPoint;

stru_GPSPOINT *str_gpsPoints;//原始经纬度
stru_GPSPOINT *str_PlanarPoints;//平移到原点并转换为米单位坐标
stru_GPSPOINT *str_outLinePoints;//取出的边界点坐标

int gpsPoints_Index=0;
int outLinePoints_Index=0;
double ShouGeLen=0;
double LengMaxx;
double LengMaxy;
double currentLatitude=0;
double currentLongitude=0;
int CaliAreaFlag=1;//为0实时连续计算面积，为1最终面积计算
int AddPointFlag=0;//为0不加点，为1加点
double BottomMaxy;

extern SysInf  SysInfor;


void A40_atuoCalculateDistances(double *total_distance, double lat, double lng, unsigned int depth, double bearing, double *oldBearing);
double caculate_autodistance(stru_GPSPOINT Autopoints[], int *index, double bearing, double *oldBearing);
double ClearPointsDistance(stru_GPSPOINT Autopoints[], int *index);
unsigned char IsChangedBearing(double originBearing, double currentBearing, double *lastBearing);
void a40_recordAutoPoints(stru_GPSPOINT Autopoints[], int *index, double lat, double lng, unsigned int depth, double bearing);



static void gpsPoint2planarPoint(stru_GPSPOINT gpsPoint[], unsigned int gpsPoints_Index, stru_GPSPOINT plannrPoint[]);
stru_GPSPOINT getCorePoint(stru_GPSPOINT planePoint[], int planePointIndex);
stru_peakPoint getPeakPoint(stru_GPSPOINT planePoint[], int planePointIndex);
double getAreaOfTravel(double distance, double width_m);
double getTotalDistance(stru_GPSPOINT planePoint[], unsigned int planePointIndex);
double getTotalDistance(stru_GPSPOINT planePoint[], unsigned int planePointIndex);
double GetDistanceInPlant(double x1, double y1, double x2, double y2);
static void CalBottomPiont(stru_GPSPOINT PlanarPoints[], unsigned int planarIndex, int getMaxPointNum, stru_GPSPOINT outLine[], unsigned int *outlineIndex);
static void CalLeftPiont(stru_GPSPOINT PlanarPoints[], unsigned int planarIndex, int getMaxPointNum, stru_GPSPOINT outLine[], unsigned int *outlineIndex);
static void CalRightPiont(stru_GPSPOINT PlanarPoints[], unsigned int planarIndex, int getMaxPointNum, stru_GPSPOINT outLine[], unsigned int *outlineIndex);
static void CalTopPiont(stru_GPSPOINT planarPoints[], unsigned int planarIndex, int getMaxPointNum, stru_GPSPOINT outLine[], unsigned int *outlineIndex);
double GetOffsetAreaForAntennaPosition(stru_GPSPOINT outLinePoints[], unsigned int index, unsigned int width);
static double getAreaOfGpsOutline(stru_GPSPOINT str_PlanarPoints[], unsigned int gpsPoints_Index, unsigned int width);
static double CaliArea(stru_GPSPOINT pPoint[], int index);
double GetDistance(double lat1, double lng1, double lat2, double lng2);
double getBearing(double bearing1, double bearing2);
void getOutLinePoint(stru_GPSPOINT PlanarPoints[], unsigned int planarIndex, unsigned int width, stru_GPSPOINT outLine[], unsigned int *outLineIndex);

#define EARTH_RADIUS 6378.137
#define PI 3.1415926535897932384626433832795

double rad(double d);
double roundx(double x);



double autoMuDistance = 0, oldBearing = 0;
double RawGpsDataX[3];
double RawGpsDataY[3];
int RawGpsDataIndex=0, autoGpsDataIndex = 0;
static stru_GPSPOINT autoGpsData[MAX_AUTO_POINTS];
static int invalid_gps_counter = 0; //空跑值，深度无效的gps距离
double getAutoDistance(int harvesterState, int clean_all_data) {//clean_all_data:按钮按下时候属于手动计亩，这时候计算并清除自动计亩所有信息
    int i;
    if (autoGpsDataIndex < 3) {
        write_log_to_sdcard(LOG_TXT_PATH, "autoGpsDataIndex=%d 小于3条 忽略本次计算", autoGpsDataIndex);
        autoGpsDataIndex = 0;
        return autoMuDistance;
    }
    write_log_to_sdcard(LOG_TXT_PATH, "%d 个记录点", autoGpsDataIndex);
    for (i = 0; i < autoGpsDataIndex; i++) {
        write_log_to_sdcard(LOG_TXT_PATH, "(%.06f, %.06f)\r\n", ((long)(autoGpsData[i].x*1000000))/1000000.0f, ((long)(autoGpsData[i].y*1000000))/1000000.0f);
    }

    double len = 0;
	for (i = 0; i < autoGpsDataIndex - 1; i++) {
		if (autoGpsData[i].valid && autoGpsData[i + 1].valid)
			len += GetDistance(autoGpsData[i].x, autoGpsData[i].y, autoGpsData[i + 1].x, autoGpsData[i + 1].y);
	}
    //SerialPortDataProcessView.write_log_to_sdcard(LOG_TXT_PATH, SerialPortDataProcessView.GPS_DATA_PATH, "计算距离 起点:(" + ((long)(autoGpsData[0].x*1000000))/1000000.0f +
    //   ", " + ((long)(autoGpsData[0].y*1000000))/1000000.0f + ") 终点(" + ((long)(autoGpsData[3].x*1000000))/1000000.0f + " ," +
    //   ((int)(autoGpsData[3].y*1000000))/1000000.0f + ")\r\n");
    autoMuDistance += len;
    write_log_to_sdcard(LOG_TXT_PATH, "单长度为%d 总长度为%d", len, autoMuDistance);
    write_gps_info("单长度为%d 总长度为%d", len, autoMuDistance);
    if (harvesterState && (!clean_all_data)) { //割麦子状态，拐角点为新的起点;否则等待割麦子状态才算新的起点
        autoGpsData[0].x = RawGpsDataX[2];
        autoGpsData[0].y = RawGpsDataY[2];
        autoGpsData[0].bearing = oldBearing;
        autoGpsDataIndex = 1;
        write_log_to_sdcard(LOG_TXT_PATH, "新起点(%f, %f) 方向:%f", ((long)(autoGpsData[0].x*1000000))/1000000.0f, ((long)(autoGpsData[0].y*1000000))/1000000.0f, ((long)(autoGpsData[0].bearing*100))/100.0f);
    } else {
        autoGpsDataIndex = 0;
        write_log_to_sdcard(LOG_TXT_PATH, "新起点:0");
    }
	LOGE("autoMuDistance = %f", autoMuDistance);
    return autoMuDistance;
}

void AndroidGpsProcess(double x,double y, int buttonStart, double bearing, double speed)
{
    //LOGE("%s %d", __func__, __LINE__);
    int valid = g_depth > 150; //深度有效值
	int harvesterState = g_depth > 100 && speed > 1; //自动计亩有效条件
	//if (harvesterState == 1) {
	//write_log_to_sdcard(LOG_TXT_PATH, "自动测亩状态：%d    速度：%f", harvesterState, speed);
		//LOGE("自动测亩状态：%d    速度：%f", harvesterState, speed);
	//}

    if (x==0 && y==0)
        return;
    //if((currentLatitude==x && currentLongitude==y))//过滤重复点
    //    return;
    currentLatitude=x;
    currentLongitude=y;
    if(RawGpsDataIndex<3) {
        RawGpsDataX[RawGpsDataIndex]=x;
        RawGpsDataY[RawGpsDataIndex]=y;
        RawGpsDataIndex++;
        return;
    } 
    //过滤比前后点误差值超过100米的点
    if(fabs(RawGpsDataX[1]-RawGpsDataX[0])>0.001&&fabs(RawGpsDataX[1]-RawGpsDataX[2])>0.001)
        RawGpsDataX[1]=RawGpsDataX[0];
    if(fabs(RawGpsDataY[1]-RawGpsDataY[0])>0.001&&fabs(RawGpsDataY[1]-RawGpsDataY[2])>0.001)
        RawGpsDataY[1]=RawGpsDataY[0];
    RawGpsDataX[0]=RawGpsDataX[1];
    RawGpsDataX[1]=RawGpsDataX[2];
    RawGpsDataX[2]=x;

    RawGpsDataY[0]=RawGpsDataY[1];
    RawGpsDataY[1]=RawGpsDataY[2];
    RawGpsDataY[2]=y;
    //if (buttonStart && (g_depth > 50 || ultra_error_flag) ) {//按钮按下，手动计亩，保存gps数据，仅用于下次系统重启时候读取计算面积
    if (buttonStart) {//按钮按下，手动计亩，保存gps数据，仅用于下次系统重启时候读取计算面积
        str_gpsPoints[gpsPoints_Index].x = x;
        str_gpsPoints[gpsPoints_Index].y = y;
        str_gpsPoints[gpsPoints_Index].valid = valid;
        if (gpsPoints_Index < (GPS_MAX_DATA - 2)) {
            stru_GPSPOINT *data = malloc(sizeof(stru_GPSPOINT));
            memcpy((void *)data, &str_gpsPoints[gpsPoints_Index++], sizeof(stru_GPSPOINT));
            add_offline_data_list(&gps_area_data_head, (void *)data, sizeof(stru_GPSPOINT));
        }
		if (g_manualDepthCounter < GPS_MAX_DATA - 1)
			manualDepthBuff[g_manualDepthCounter++] = g_depth;
       
    }
	A40_atuoCalculateDistances(&g_totalDistances_auto, x, y, g_depth, bearing, &g_oldBearing);
   
}

/*******************************************************************************/
int autoDepthBuff[GPS_MAX_DATA];
unsigned int  g_depthCounter = 0;
//自动计算线长	
void A40_atuoCalculateDistances(double *total_distance, double lat, double lng, unsigned int depth, double bearing, double *oldBearing)
{
	
	if (depth<AUTO_VALID_DEPTH_MM ) {  //抬铲,没干活
		if(g_continuousNoWorkPointNums<20)
			g_continuousNoWorkPointNums++;
	}
	else {//落铲,干活
		LOGE("autoGpsDataIndex =%d ", autoGpsDataIndex);
		g_continuousNoWorkPointNums = 0;
		if (auto_area_start_flags == 0) {//提取开始时间
			auto_area_start_flags = 1;
			gettimeofday(&auto_area_start_time, (struct timezone *) NULL);
			gettimeofday(&auto_calc_tv1, (struct timezone *) NULL);
		}
		a40_recordAutoPoints(autoGpsData, &autoGpsDataIndex, lat, lng, depth, bearing);//记录点
		if (g_depthCounter < GPS_MAX_DATA - 1)
			autoDepthBuff[g_depthCounter++] = depth;
		*total_distance += caculate_autodistance(autoGpsData, &autoGpsDataIndex, bearing, oldBearing);//累计线长
		LOGE("累计线长：%f\r\n",*total_distance);
	}

	//LOGE("auto_area_start_flags =%d ", auto_area_start_flags);
	//LOGE("g_continuousNoWorkPointNums =%d ", g_continuousNoWorkPointNums);
	
	if (auto_area_start_flags == 1 )//开始作业后判断
	{
		gettimeofday(&auto_calc_tv2, (struct timezone *) NULL);//取出当前秒数
		int diff_minutes = abs(SEC_DIFF(auto_calc_tv1, auto_calc_tv2))/60;//已经工作的时长，分钟
		if (g_continuousNoWorkPointNums > 5 || diff_minutes> 30 ) {	//抬铲超过5个点，工作时长大于30分钟，结算线长

			*total_distance = *total_distance + ClearPointsDistance(autoGpsData, &autoGpsDataIndex);//结算线长
			LOGE("累计线长：%f\r\n", *total_distance);
			autoCalArea(*total_distance, &auto_area_start_time);//计算自动面积、深度、合格率等信息
			g_depthCounter = 0;
			autoGpsDataIndex = 0;
			auto_area_start_flags = 0;
			*total_distance = 0;
		}

	}

}

void stopAutoArea() {
	g_totalDistances_auto = g_totalDistances_auto + ClearPointsDistance(autoGpsData, &autoGpsDataIndex);//结算线长
	LOGE("累计线长：%f\r\n", g_totalDistances_auto);
	autoCalArea(g_totalDistances_auto, &auto_area_start_time);//计算自动面积、深度、合格率等信息
	g_depthCounter = 0;
	autoGpsDataIndex = 0;
	auto_area_start_flags = 0;
	g_totalDistances_auto = 0;
}

double caculate_autodistance(stru_GPSPOINT Autopoints[], int *index,double bearing,double *oldBearing)
{
	int i = 0;
	double distance = 0;
	if (*index>3) {
		if (IsChangedBearing(Autopoints[0].bearing,bearing,oldBearing) == 1) {//方向发生改变,结算一次直线距离，并确定下一点
			
			distance = ClearPointsDistance(Autopoints, index);
			LOGE("改变方向，直线距离:%f",distance);
			if (Autopoints[*index - 1].valid == 1) {//如果当前点状态位作业，确定该点为新直线的起点;否则等待状态变为“作业”再确定新起点
				Autopoints[0].x = Autopoints[*index - 1].x;
				Autopoints[0].y = Autopoints[*index - 1].y;
				Autopoints[0].valid = Autopoints[*index - 1].valid;
				Autopoints[0].bearing = Autopoints[*index - 1].bearing;
				*index = 1;
			}
			else {
				*index = 0;
			}

		}

	}
	return distance;
}

double ClearPointsDistance(stru_GPSPOINT Autopoints[], int *index)
{
	double distance = 0;
	int i = 0;
	if (*index > 2) {
		for (i = 0; i < (*index - 1); i++) {
			if (Autopoints[i].valid && Autopoints[i + 1].valid)
				distance += GetDistance(Autopoints[i].x, Autopoints[i].y, Autopoints[i + 1].x, Autopoints[i + 1].y);
		}
	}
	return distance;
}

unsigned char IsChangedBearing(double originBearing,double currentBearing,double *lastBearing)
{
	unsigned char isChanged = 0;
	if ((getBearing(originBearing, currentBearing) >= 40) && (getBearing(originBearing, *lastBearing) >= 40)) {              //连续两个角度大于40度即认为转弯

		write_log_to_sdcard(LOG_TXT_PATH, "终点 起始角:%f 拐角1:%f 拐角2:%f", ((long)(originBearing * 100)) / 100.0f, ((long)(*lastBearing * 100)) / 100.0f,((long)(currentBearing * 100)) / 100.0f);
		write_log_to_sdcard(LOG_TXT_PATH, "角度变化,计算一次距离");
		write_gps_info("%s", "角度变化,计算一次距离");
		isChanged = 1;
	}
	*lastBearing = currentBearing;
	return isChanged;
}



void a40_recordAutoPoints(stru_GPSPOINT Autopoints[], int *index,double lat, double lng, unsigned int depth, double bearing)
{
	int i = 0;
	int autoGpsDataIndex = *index;
	if (autoGpsDataIndex < MAX_AUTO_POINTS) {
		Autopoints[autoGpsDataIndex].x = lat;
		Autopoints[autoGpsDataIndex].y = lng;
		Autopoints[autoGpsDataIndex].bearing = bearing;
		Autopoints[autoGpsDataIndex].depth = depth;
		Autopoints[autoGpsDataIndex].valid = ( depth>AUTO_VALID_DEPTH_MM )? 1 : 0;
		autoGpsDataIndex++;
		*index = autoGpsDataIndex;
		if (autoGpsDataIndex == 1) {
			write_log_to_sdcard(LOG_TXT_PATH, "起点(%.06f, %.06f) 方向:%.02f\r\n", lat, lng, bearing);
		}
	}
	else {

		for (i = 2; i<MAX_AUTO_POINTS - 1; i++) {
			Autopoints[i].x = Autopoints[i + 1].x;
			Autopoints[i].y = Autopoints[i + 1].y;
			Autopoints[i].depth = Autopoints[i + 1].depth;
			Autopoints[i].bearing = Autopoints[i + 1].bearing;
			Autopoints[i].valid = Autopoints[i + 1].valid;
		}
		Autopoints[MAX_AUTO_POINTS - 1].x = lat;
		Autopoints[MAX_AUTO_POINTS - 1].y = lng;
		Autopoints[MAX_AUTO_POINTS - 1].bearing = bearing;
		Autopoints[MAX_AUTO_POINTS - 1].depth = depth;
		Autopoints[MAX_AUTO_POINTS - 1].valid = (depth > AUTO_VALID_DEPTH_MM ) ? 1 : 0;
	}
}

/*******************************************************************************/


//计算手动轮廓面积
double CalAreaInManual(stru_GPSPOINT gpsPoints[], unsigned int gpsIndex, stru_GPSPOINT planarPoints[], unsigned int width) //计算面积处理过程
{
	double area = 0;
	gpsPoint2planarPoint(gpsPoints, gpsIndex, planarPoints);//经纬度点转换位平面坐标点
	g_corePoint = getCorePoint(planarPoints, gpsIndex);//取出重心点
	g_peakPoint = getPeakPoint(planarPoints, gpsIndex);//峰值点
	area = getAreaOfGpsOutline(planarPoints, gpsIndex, width);//取边界点并计算面积
	return area;


}


//////---转换经纬度为米单位的平面坐标----////////
static void gpsPoint2planarPoint(stru_GPSPOINT gpsPoint[], unsigned int gpsPoints_Index, stru_GPSPOINT plannrPoint[])
{
	double Klatitude;//纬度系数，1度=?米
	double Klongitude;//经度系数,1度=?米
	double tminx, tminy;
	unsigned int i;
	if (gpsPoints_Index<2)return;


	Klatitude = GetDistance(gpsPoint[0].x + 1, gpsPoint[0].y, gpsPoint[0].x, gpsPoint[0].y);
	Klongitude = GetDistance(gpsPoint[0].x, gpsPoint[0].y + 1, gpsPoint[0].x, gpsPoint[0].y);

	tminx = gpsPoint[0].x;
	tminy = gpsPoint[0].y;

	for (i = 1; i <gpsPoints_Index; i++)//循环所有线条求极值
	{
		if (gpsPoint[i].x<tminx)tminx = gpsPoint[i].x;//求最小值
		if (gpsPoint[i].y<tminy)tminy = gpsPoint[i].y;//求最小值
	}
	for (i = 0; i <gpsPoints_Index; i++)//循环所有线条，平行到零点坐标，并转换为米单位
	{
		plannrPoint[i].x = (gpsPoint[i].x - tminx)*Klatitude;
		plannrPoint[i].y = (gpsPoint[i].y - tminy)*Klongitude;
	}

}







//////////----轮廓面积---///////////
static double getAreaOfGpsOutline(stru_GPSPOINT str_PlanarPoints[], unsigned int gpsPoints_Index, unsigned int width)
{
	double iArea = 0;
	getOutLinePoint(str_PlanarPoints, gpsPoints_Index, width, str_outLinePoints, &outLinePoints_Index);

	iArea = CaliArea(str_outLinePoints, outLinePoints_Index);//求面积
	write_log_to_sdcard(LOG_TXT_PATH, "轮廓面积：%.03f", iArea / 666.67);
	iArea += GetOffsetAreaForAntennaPosition(str_outLinePoints, outLinePoints_Index, width);
	write_log_to_sdcard(LOG_TXT_PATH, "总面积：%.03f", iArea / 666.67);
	return (iArea / 666.67);
}






//处理计算作业面积任务
double getAreaOfMoving(stru_GPSPOINT planePoint[], unsigned int planePointIndex, unsigned int width)
{
	double area_mu = 0;
	double g_totalDistance = 0;
	if (planePointIndex >= 10)//超过n个点计算距离
	{
		g_totalDistance = getTotalDistance(str_PlanarPoints, planePointIndex);
		area_mu = getAreaOfTravel(g_totalDistance, width / 100.0);
	}
	else
		area_mu = 0;
	write_log_to_sdcard(LOG_TXT_PATH, "行进面积：%.03f", area_mu);
	return area_mu;

}//
 //计算面积
double getAreaOfTravel(double distance, double width_m)
{
	double area_mu;
	area_mu = distance * width_m / 666.667;
	return area_mu;

}//


 //计算行进总距离
double getTotalDistance(stru_GPSPOINT planePoint[], unsigned int planePointIndex)
{
	unsigned int i = 0;
	double g_neighborGpsDistances = 0;
	double total = 0;
	//计算单点移动距
	for (i = 0; i < planePointIndex - 1; i++)
	{

		g_neighborGpsDistances = GetDistanceInPlant(planePoint[i].x, planePoint[i].y, planePoint[i + 1].x, planePoint[i + 1].y);

		if (g_neighborGpsDistances < 100)
			total += g_neighborGpsDistances;
	}
	return  total;

}//



 //求2个平面坐标点之间的距离
double GetDistanceInPlant(double x1, double y1, double x2, double y2)
{
	double distance = 0;
	distance = fabs(pow((x1 - x2), 2) + pow((y1 - y2), 2));
	return sqrt(distance);
}



stru_peakPoint getPeakPoint(stru_GPSPOINT planePoint[], int planePointIndex)
{

	stru_peakPoint peak;// 
	unsigned  int i = 0;
	peak.x_max = planePoint[0].x;
	peak.x_min = planePoint[0].x;
	peak.y_max = planePoint[0].y;
	peak.y_min = planePoint[0].y;

	for (i = 1; i < planePointIndex; i++)
	{
		if (peak.x_max < planePoint[i].x) peak.x_max = planePoint[i].x;
		if (peak.x_min > planePoint[i].x) peak.x_min = planePoint[i].x;

		if (peak.y_max < planePoint[i].y) peak.y_max = planePoint[i].y;
		if (peak.y_min > planePoint[i].y) peak.y_min = planePoint[i].y;

	}
	peak.length_x = peak.x_max - peak.x_min;
	peak.length_y = peak.y_max - peak.y_min;
	write_log_to_sdcard(LOG_TXT_PATH, "Xmax:%.03f,Xmin:%.03f,Xlen:%.03f", peak.x_max, peak.x_min, peak.length_x);
	write_log_to_sdcard(LOG_TXT_PATH, "Ymax:%.03f,Ymin:%.03f,Ylen:%.03f", peak.y_max, peak.y_min, peak.length_y);
	return peak;

}
stru_GPSPOINT getCorePoint(stru_GPSPOINT planePoint[], int planePointIndex)
{
	double X = 0, Y = 0;
	unsigned int i = 0;
	stru_GPSPOINT core;
	for (i = 0; i < planePointIndex; i++)
	{
		X += planePoint[i].x;
		Y += planePoint[i].y;
	}
	core.x = X / planePointIndex;
	core.y = Y / planePointIndex;
	return core;

}





////////-----多边形求面积-----/////////////
static double CaliArea(stru_GPSPOINT pPoint[], int index)//多边形求面积
{
	double iArea = 0;
	double p1x, p1y, p2x, p2y;
	unsigned int  i;
	pPoint[index].x = pPoint[0].x;
	pPoint[index].y = pPoint[0].y;
	iArea = 0;
	for (i = 0; i <index; i++)//循环所有线条
	{
		p1x = pPoint[i].x;
		p1y = pPoint[i].y;
		p2x = pPoint[(i + 1) % index].x;
		p2y = pPoint[(i + 1) % index].y;
		iArea = iArea + (p1x*p2y - p2x*p1y);
	}
	return abs(0.5*iArea);
}


////----求最高连续点-----/////////////
static double TopMiny;
static void CalTopPiont(stru_GPSPOINT planarPoints[], unsigned int planarIndex, int getMaxPointNum, stru_GPSPOINT outLine[], unsigned int *outlineIndex)
{
	double ttmaxy = 0;
	double ttmaxx = 0;
	unsigned int i, j, k;
	double dx;
	unsigned int index = *outlineIndex;
	k = 0;
	dx = g_peakPoint.length_x / getMaxPointNum;
	for (i = 0; i<getMaxPointNum; i++)//分割后循环
	{
		ttmaxy = g_corePoint.y;
		for (j = 0; j <planarIndex; j++)//循环所有线条
		{
			if (planarPoints[j].x >= i*dx&&planarPoints[j].x<(i + 1)*dx)
			{
				if (ttmaxy<planarPoints[j].y)
				{
					ttmaxy = planarPoints[j].y;
					ttmaxx = planarPoints[j].x;
				}
			}
		}
		if (ttmaxy != g_corePoint.y)//添加新点
		{
			outLine[*outlineIndex].x = ttmaxx;
			outLine[*outlineIndex].y = ttmaxy;
			*outlineIndex = (*outlineIndex) + 1;
			k++;
		}
	}
	//去掉3点间，中间最低点
	if (k<3)return;
	for (i = 2; i<k; i++)
	{
		if (outLine[*outlineIndex - i].y<outLine[*outlineIndex - i - 1].y
			&&outLine[*outlineIndex - i].y<outLine[*outlineIndex - i + 1].y)
			outLine[*outlineIndex - i].y = outLine[*outlineIndex - i + 1].y;
	}
	TopMiny = outLine[*outlineIndex - 1].y;
	for (i = 1; i<k; i++)
	{
		if (TopMiny>outLine[*outlineIndex - i].y)TopMiny = outLine[*outlineIndex - i].y;
	}
}

//////----求最右边连续点-----/////////
static double RightMinx;
static void CalRightPiont(stru_GPSPOINT PlanarPoints[], unsigned int planarIndex, int getMaxPointNum, stru_GPSPOINT outLine[], unsigned int *outlineIndex)
{
	double ttmaxy = 0;
	double ttmaxx = 0;
	unsigned int i, j, k;
	double dy;
	dy = g_peakPoint.length_y / getMaxPointNum;
	k = 0;
	for (i = 0; i<getMaxPointNum; i++)//分割后循环
	{
		ttmaxx = g_corePoint.x;
		for (j = 0; j <planarIndex; j++)//循环所有线条
		{
			if (PlanarPoints[j].y <= (getMaxPointNum - i)*dy&&PlanarPoints[j].y>(getMaxPointNum - (i + 1))*dy)
			{
				if (ttmaxx<str_PlanarPoints[j].x && (TopMiny - 1)>str_PlanarPoints[j].y)
				{
					ttmaxy = PlanarPoints[j].y;
					ttmaxx = PlanarPoints[j].x;
				}
			}
		}
		if (ttmaxx != g_corePoint.x)
		{
			outLine[*outlineIndex].x = ttmaxx;
			outLine[*outlineIndex].y = ttmaxy;
			(*outlineIndex)++;
			k++;
		}
	}

	//去掉3点间，中间最左点
	if (k<3)return;
	for (i = 2; i<k; i++)
	{
		if (outLine[*outlineIndex - i].x<outLine[*outlineIndex - i - 1].x
			&&outLine[*outlineIndex - i].x<outLine[*outlineIndex - i + 1].x)
			outLine[*outlineIndex - i].x = outLine[*outlineIndex - i + 1].x;
	}

	RightMinx = outLine[*outlineIndex - 1].x;
	for (i = 1; i<k; i++)
	{
		if (RightMinx>outLine[*outlineIndex - i].x)RightMinx = outLine[*outlineIndex - i].x;
	}
}

//////----求最低边连续点-----/////////
static void CalBottomPiont(stru_GPSPOINT PlanarPoints[], unsigned int planarIndex, int getMaxPointNum, stru_GPSPOINT outLine[], unsigned int *outlineIndex)
{
	double tminx, tminy;
	unsigned int i, j, k;
	double dx;
	k = 0;
	tminx = tminy = 100000;//100km
	dx = g_peakPoint.length_x / getMaxPointNum;
	for (i = 0; i<getMaxPointNum; i++)//分割后循环
	{
		tminy = g_corePoint.y;
		for (j = 0; j <planarIndex; j++)//循环所有线条
		{
			if (PlanarPoints[j].x <= (getMaxPointNum - i)*dx&&PlanarPoints[j].x>(getMaxPointNum - (i + 1))*dx)
			{
				if (tminy>str_PlanarPoints[j].y && (RightMinx - 1) > str_PlanarPoints[j].x)
				{
					tminy = PlanarPoints[j].y;
					tminx = PlanarPoints[j].x;
				}
			}
		}
		if (tminy != g_corePoint.y)
		{
			outLine[*outlineIndex].x = tminx;
			outLine[*outlineIndex].y = tminy;
			(*outlineIndex)++;
			k++;
		}
	}
	//去掉3点间，中间最高点
	if (k<3)return;
	for (i = 2; i<k; i++)
	{
		if (outLine[*outlineIndex - i].y > outLine[*outlineIndex - i - 1].y 	&& outLine[*outlineIndex - i].y>outLine[*outlineIndex - i + 1].y)
			outLine[*outlineIndex - i].y = outLine[*outlineIndex - i + 1].y;
	}

	BottomMaxy = outLine[*outlineIndex - 1].y;
	for (i = 1; i<k; i++)
	{
		if (BottomMaxy<outLine[*outlineIndex - i].y)BottomMaxy = outLine[*outlineIndex - i].y;
	}
}

//////----求最左边连续点-----/////////
static void CalLeftPiont(stru_GPSPOINT PlanarPoints[], unsigned int planarIndex, int getMaxPointNum, stru_GPSPOINT outLine[], unsigned int *outlineIndex)
{
	double tminx, tminy;
	unsigned int i, j, k;
	double dy;
	k = 0;
	tminx = tminy = 100000;//100km
	dy = g_peakPoint.length_y / getMaxPointNum;
	for (i = 0; i<getMaxPointNum; i++)//分割后循环
	{
		tminx = g_corePoint.x;
		for (j = 0; j <planarIndex; j++)//循环所有线条
		{
			if (PlanarPoints[j].y >= i*dy&&PlanarPoints[j].y<(i + 1)*dy)
			{

				if (tminx>PlanarPoints[j].x && (BottomMaxy + 1)<PlanarPoints[j].y && (TopMiny - 1)>PlanarPoints[j].y)
				{
					tminy = PlanarPoints[j].y;
					tminx = PlanarPoints[j].x;
				}
			}
		}
		if (tminx != g_corePoint.x)
		{
			outLine[*outlineIndex].x = tminx;
			outLine[*outlineIndex].y = tminy;
			(*outlineIndex)++;
			k++;
		}
	}
	if (k<3)return;
	//去掉3点间，中间最右点
	for (i = 2; i<k; i++)
	{
		if (outLine[*outlineIndex - i].x>outLine[*outlineIndex - i - 1].x
			&&outLine[*outlineIndex - i].x>outLine[*outlineIndex - i + 1].x)
			outLine[*outlineIndex - i].x = outLine[*outlineIndex - i + 1].x;
	}
}



//得到因GPS天线安装位置偏差带来的面积误差
double GetOffsetAreaForAntennaPosition(stru_GPSPOINT outLinePoints[], unsigned int index, unsigned int width)//outLinePoints
{
	unsigned int  i = 0;
	double x1;
	double x2;
	double y1;
	double y2;
	double area = 0;
	double distances = 0;
	double QuYangLength = 0;

	for (i = 0; i<index - 1; i++)
	{
		x1 = outLinePoints[i].x;
		x2 = outLinePoints[i + 1].x;
		y1 = outLinePoints[i].y;
		y2 = outLinePoints[i + 1].y;
		distances = sqrt(pow((x1 - x2), 2) + pow((y1 - y2), 2));
		//cout << "distances   "<< i<<"  :  " << distances << endl;
		QuYangLength += distances;
	}

	area = width*QuYangLength / 200;
	write_log_to_sdcard(LOG_TXT_PATH, "补偿面积：%.03f", area / 666.67);
	return area;
}


void getOutLinePoint(stru_GPSPOINT PlanarPoints[], unsigned int planarIndex, unsigned int width, stru_GPSPOINT outLine[], unsigned int *outLineIndex)
{

	int minPointsPerLine = 6;//每条线上取的点数
	double  gapInPoint;//每条线的取点间距,机具宽度width，单位厘米，取点间距gapInPoint，单位米 ，相当于 3 倍机具宽度,最小5米
	if (3 * width / 100 > 5) {
		gapInPoint = 3 * width / 100;
	}
	else
	{
		gapInPoint = 5;

	}
	//x,y,方向取得的点数
	unsigned int  x_getNum = (minPointsPerLine > (g_peakPoint.length_x / gapInPoint)) ? minPointsPerLine : (g_peakPoint.length_x / gapInPoint);
	unsigned int  y_getNum = (minPointsPerLine > (g_peakPoint.length_y / gapInPoint)) ? minPointsPerLine : (g_peakPoint.length_y / gapInPoint);


	if (planarIndex < 20) return;//少于20个点，不计算面积
	memset(outLine, 0, sizeof(outLine));
	*outLineIndex = 0;

	CalTopPiont(PlanarPoints, planarIndex, x_getNum, outLine, outLineIndex);//顶边线
	CalRightPiont(PlanarPoints, planarIndex, x_getNum, outLine, outLineIndex);//右边线
	CalBottomPiont(PlanarPoints, planarIndex, x_getNum, outLine, outLineIndex);//下边线
	CalLeftPiont(PlanarPoints, planarIndex, x_getNum, outLine, outLineIndex);//左边线

}


  double roundx(double x) {
	return ((int)(x + 0.5));
}

 double rad(double d) {
	return d * PI / 180.0;
}

/////--------求两点间距离---------/////////////
 double GetDistance(double lat1, double lng1, double lat2, double lng2)
{
	double radLat1 = rad(lat1);
	double radLat2 = rad(lat2);
	double a = radLat1 - radLat2;
	double b = rad(lng1) - rad(lng2);
	double c = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1)*cos(radLat2)*pow(sin(b / 2), 2)));

	c = c * EARTH_RADIUS;
	c = roundx(c * 1000000) / 1000;//千米转换为米，取小数点3位
	return c;
}

 double getBearing(double bearing1, double bearing2)
 {
	double bearing = fabs(bearing1 - bearing2);
	if (bearing > 180) {
		bearing = 360 - bearing;
	}
	return bearing;
}
